/*
 * trajectory.hpp
 *
 *  Created on: 2020年12月2日
 *      Author: 刘成吉
 */

#ifndef MODULES_TRAJECTORY_TRAJECTORY_HPP_
#define MODULES_TRAJECTORY_TRAJECTORY_HPP_

#include "system/system.hpp"
#include "userlib/userlib.hpp"
#include "target.hpp"
#include "minimum_snap.hpp"

#ifdef __cplusplus

#include <Eigen>

using namespace std;
using namespace Eigen;

#define FLY_TIME            500
#define ACCEL               1.0f
#define MAX_COM_SPEED       4.0f
#define BOUNDARY_DISTANCE   2.0f
#define DEFAULT_HOVER_TIME  500

#define ACCEL_VERTICAL               0.5f
#define MAX_COM_SPEED_VERTICAL       1.0f
#define BOUNDARY_DISTANCE_VERTICAL   1.0f

#define PILOT_HEIGHT        1.0f
#define TAKE_OFF_HEIGHT     1.0f
#define TRAJECTORY_CONTROL_DT 0.01f

enum  Trajectory_mod
{
	Hover,                      //0
	Point_to_point,		    //1
	Circle,			    //2
	Mixture,		    //3
	Test,			    //4
	RTL,                        //5
	Dubins,
	MinimumSnap,
	False
};

enum HeliStep
{
	hoverStep = 1,
	turnHeadStep = 2,
	speedUpStep = 3,
	speedDownStep = 4,
	brakeDownStep = 8,
	circleStep = 9
};

enum circleDir{
	leftCircle = 2,
	rightCircle = 1
};

enum  Dubins_Set
{
	LSL,                      //0
	LSR,		    //1
	RSL,			    //2
	RSR,		    //3
	RLR,			    //4
	LRL,                        //5
	FALSE
};

enum minimumSnapStep{
	msInit = 1,
	msTurnHeadStep = 2,
	msFeedingPointStep = 3,
	msBrakeDownStep = 4,
	msHoverStep = 5
};

typedef struct{
	float x;
	float y;
	float yaw_dubins;
}Dubins_Target;

typedef struct{
	float t; //第一次旋转的角度
	float p; //第二次旋转的角度/线长
	float q; //第三次旋转的角度
	float dist; //总dubins曲线路程
	enum  Dubins_Set kind; //dubins曲线的种类，包括6种
}Dubins_Curve;



class TRAJECTORY:public TARGET
{
public:
	float        top_speed_body[3];//记录飞机三轴的实际能达到的最大速度，[0]表示横向，[2]表示纵向
	float        how_long_body[3] ; //记录飞机当前期望点与下一个给定点之间的距离，[0]表示横向，[2]表示纵向
	float        next_target_ned[4]; //
	float        end_heading;
	float        hover_time;
	float        hover_ned[3];      //,
	float        cur_target_vel_body[3]; //机头坐标系下x、y、z轴的速度
	float        cur_target_vel_ned[3]; //飞机在NED坐标系下，在N、E、D三个方向上的分速度
	float        cur_target_pos_ned[3]; //飞机NED坐标
	float        cur_target_acc_ned[3]; //飞机NED加速度，当前仅有minimum snap曲线有用到
	float        com_pqr[3];
	float        com_pqr_ned[3];
	float        com_theta[3];
	HeliStep     heli_step;
	int          isturn;
	float        yaw_com_theta;
	enum Trajectory_mod trajectory_mod;
	float        sector_angle;    //
	float        circle_velocity; //
	float 		 helix_velocity;  //
	float		 circle_radius;	 //
	float        circle_angle;
	float        circle_radius_test;
	float 		 next_next_target[4];
	enum circleDir circle_turn;
	float        distance_line; //
	float        distance_vertical;//
	int 		 vertical_speed_direction;//
	int 		 RTL_Orignal_flag; //RTL
	int 		 RTL_Orignal_update_flag;//RTL
	int 	     RTL_First_switch;
	int 		 first_receive_target;
	int 		 turn_accel;
	bool         turn_accel_flag;
	int          first_entry_brake;
	float        brake_distance;
	int          first_entry_circle;
	int			 cushion_flag;
	bool         circle_finish_flag;
	float 	     yaw_command;
	int          circleNum;

	//dubinsCurve
	bool 		 dubins_target_init;//初始化dubins目标点标志位
	bool         dubins_mission_moving;//正在执行dubins曲线标志位
	bool         clearFlag;
	int          dubins_idx;
	u8           dubins_step;
	bool         yawAutoSetFlag; //做dubins曲线时，若该标志位为true，则不用地面站上传期望角度
	Dubins_Target  dubins_target_buffer[MAX_QUEUE_SIZE];//dubins目标单队列
	u8			 dubins_segment;//规划完毕的dubins曲线段个数
	u8			 dubins_segment_moving;//当前正在执行的dubins曲线段
	Dubins_Curve dubins_curve_buffer[MAX_QUEUE_SIZE - 1];//规划完毕的dubins曲线参数

	//minimumSnap
	MatXd polynomial_coeff_x; //px的8个多项式系数
	MatXd polynomial_coeff_y;
	MatXd polynomial_coeff_vx; //vx的7个多项式系数
	MatXd polynomial_coeff_vy;
	MatXd polynomial_coeff_ax; //ax的6个多项式系数
	MatXd polynomial_coeff_ay;
	VecXd allocateTime;
	float minimumSnapExeTime; //一次minimum snap规划时间
	bool minimumSnapPlanningFinish; //minimum snap 规划完成标志位
	int segmentNum; //minimum snap的轨迹段数
	minimumSnapStep minimumSnapStepFlag;
	int curSegment; //minimum snap当前所在的轨迹编号
	double curTime; //在curSegment上的当前时间
	float startPoint[2]; //minimum snap的起点

public:
	void Trajectory_Reset(void);
	void Trajectory_Step(void);
	void Trajectory_Point_to_Point_Mode_Step(void);
	void Trajectory_Hover_Mode_Step(void);
	void Trajectory_Point_to_point_Mode_Step(void);
	void Trajectory_Circle_Mode_Step(void);
	void Trajectory_Mixture_Mode_Step(void);
	void Trajectory_Test_Mode_Step(void);
	void Trajectory_RTL_Mode_Step(void);
	void Trajectory_Brake_Down_Step(void);
	void Trajectory_Schedule(int max_speed);

	void Trajectory_Hover_Step(void);
	void Trajectory_Circle_Step(float _circle_velocity, float _radius,float _helix_velocity, float _angle);
	bool Trajectory_Target_Update(void);
	void Target_Plus_Hover_Point(void);
	void Trajectory_Turn_Heading(float * end_heading_point);
	void Trajectory_Turning_Head_Step(void);
	void Trajectory_Speed_Up_Step(void);
	void Trajectory_Speed_Down_Step(void);
	void Trajectory_Distance_Calculate(void);
	void Trajectory_Speed_Calculate(int max_com_speed);
	void Trajectory_Target_Receive(void);

	void Trajectory_Dubins_Mode_Step(void);
	void Dubins_Planning(void);
	void Trajectory_Dubins_Circle_Step(float _circle_velocity, float _radius,u8 left_right, float _angle);
	void Trajectory_Dubins_Straight_Step(void);
	void Dubins_Cal(Dubins_Target start_point,Dubins_Target end_point,float radius,Dubins_Curve *result);
	void Trajectory_Dubins_LSL_Step(void);
	void Trajectory_Dubins_LSR_Step(void);
	void Trajectory_Dubins_RSL_Step(void);
	void Trajectory_Dubins_RSR_Step(void);
	void Trajectory_Dubins_RLR_Step(void);
	void Trajectory_Dubins_LRL_Step(void);
	void dubins_LSL(float alpha,float beta,float d,Dubins_Curve *result);
	void dubins_LSR(float alpha,float beta,float d,Dubins_Curve *result);
	void dubins_RSL(float alpha,float beta,float d,Dubins_Curve *result);
	void dubins_RSR(float alpha,float beta,float d,Dubins_Curve *result);
	void dubins_RLR(float alpha,float beta,float d,Dubins_Curve *result);
	void dubins_LRL(float alpha,float beta,float d,Dubins_Curve *result);

	void minimumSnap_Planning(void);
	void Trajectory_MinimumSnap_Mode_Step(void);


protected:
	eskf_msg eskf;
	Control_output_msg control_output;
};

extern TRAJECTORY trajectory;

#endif
#endif /* MODULES_TRAJECTORY_TRAJECTORY_HPP_ */
